#include "NPointCalibration.h"


//创建标定矩阵
//入参:
//px: 原始点X
//py: 原始点y
//qx: 原始点x
//qy: 原始点y
//出参:
//matrix: 标定矩阵
bool DhfLib::NPointCalibration::CreateCalibMatrix(std::vector<double>& px, std::vector<double>& py, std::vector<double>& qx, std::vector<double>& qy, std::vector<double>& matrix)
{
    //参数校验,确保入参无误
    int paramLength = px.size();
    if (paramLength == 0)
    {
        return false;
    }
    if ((paramLength != py.size()) ||
        (paramLength != qx.size()) ||
        (paramLength != qy.size()))
    {
        return false;
    }

    try
    {
        matrix.clear();

        double p[4][4] = { 0 };
        double q[3] = { 0 };
        double q2[3] = { 0 };
        double A[4][4] = { 0 };
        double A2[4][4] = { 0 };
        double B[2][4] = { 0 };
        double B2[2][4] = { 0 };
        double C[1][4] = { 0 };
        double C2[1][4] = { 0 };

        for (size_t i = 0; i < paramLength; i++)
        {
            p[0][0] += px[i] * px[i];
            p[0][1] += px[i] * py[i];
            p[0][2] += px[i];
            p[1][0] = p[0][1];
            p[1][1] += py[i] * py[i];
            p[1][2] += py[i];
            p[2][0] = p[0][2];
            p[2][1] = p[1][2];
            p[2][2] += 1;
        }

        for (size_t i = 0; i < paramLength; i++)
        {
            q[0] += -px[i] * qx[i];
            q[1] += -py[i] * qx[i];
            q[2] += -qx[i];
            q2[0] += -px[i] * qy[i];
            q2[1] += -py[i] * qy[i];
            q2[2] += -qy[i];
        }

        for (size_t i = 0; i < 3; i++)
        {
            A[i][0] = p[i][0];
            A[i][1] = p[i][1];
            A[i][2] = p[i][2];
            A[i][3] = q[i];

            A2[i][0] = p[i][0];
            A2[i][1] = p[i][1];
            A2[i][2] = p[i][2];
            A2[i][3] = q2[i];
        }

        double m2 = fabs(A[1][2]);
        double b = (A[0][2] * A[1][2]) > 0 ? -1 : 1;
        double m3 = b * fabs(A[0][2]);
        for (size_t i = 0; i < 4; i++)
        {
            B[0][i] = A[0][i] * m2 + A[1][i] * m3;
        }

        double m0 = fabs(A[2][2]);
        double k = A[0][2] * A[2][2] > 0 ? -1 : 1;
        double mi = k * fabs(A[0][2]);

        for (size_t i = 0; i < 4; i++)
        {
            B[1][i] = A[0][i] * m0 + A[2][i] * mi;
        }

        double m5 = fabs(B[1][1]);
        double t = B[0][1] * B[1][1] > 0 ? -1 : 1;
        double m1 = t * fabs(B[0][1]);
        for (size_t i = 0; i < 4; i++)
        {
            C[0][i] = B[0][i] * m5 + B[1][i] * m1;
        }

        //the factor of axis x
        double tRe[6] = { 0 };
        tRe[0] = (0 - C[0][3]) / C[0][0];
        tRe[1] = (0 - B[0][3] - B[0][0] * tRe[0]) / B[0][1];
        tRe[2] = (0 - A[0][3] - A[0][0] * tRe[0] - A[0][1] * tRe[1]) / A[0][2];

        //get axis y
        double n2 = fabs(A2[1][2]);
        double b22 = A2[0][2] * A2[1][2] > 0 ? -1 : 1;
        double n3 = b22 * fabs(A2[0][2]);
        for (size_t i = 0; i < 4; i++)
        {
            B2[0][i] = A2[0][i] * n2 + A2[1][i] * m3;
        }

        double n0 = fabs(A2[2][2]);
        double k2 = A2[0][2] * A2[2][2] > 0 ? -1 : 1;
        double ni = k2 * fabs(A2[0][2]);
        for (size_t i = 0; i < 4; i++)
        {
            B2[1][i] = A2[0][i] * n0 + A2[2][i] * ni;
        }

        double n5 = fabs(B2[1][1]);
        double t2 = B[0][1] * B2[1][1] > 0 ? -1 : 1;
        double n1 = t * fabs(B2[0][1]);
        for (size_t i = 0; i < 4; i++)
        {
            C2[0][i] = B2[0][i] * n5 + B2[1][i] * n1;
        }

        //the factor of axis y
        tRe[3] = (0 - C2[0][3]) / C2[0][0];
        tRe[4] = (0 - B2[0][3] - B2[0][0] * tRe[3]) / B2[0][1];
        tRe[5] = (0 - A2[0][3] - A2[0][0] * tRe[3] - A2[0][1] * tRe[4]) / A2[0][2];

        //结果校验
        for (auto item : tRe)
        {
            if (isinf(item) || isnan(item))
            {
                return false;
            }
        }

        //保存参数
        for (auto item : tRe)
        {
            matrix.push_back(item);
        }

        return true;
    }
    catch (const std::exception&)
    {
    }

    return false;
}

//标定
//入参:
//matrix: 标定矩阵
//px: 原始点X
//py: 原始点y
//出参:
//qx: 原始点x
//qy: 原始点y
bool DhfLib::NPointCalibration::Calibrate(std::vector<double>& matrix, double px, double py, double& qx, double& qy)
{
    if (matrix.size() < 6)
    {
        return false;
    }

    qx = matrix[0] * px + matrix[1] * py + matrix[2];
    qy = matrix[3] * px + matrix[4] * py + matrix[5];

    return true;
}
